/*  ----------------------------------- DSP/BIOS Headers            */
#include <std.h>
#include <gbl.h>
#include <log.h>
#include <swi.h>
#include <sys.h>
#include <tsk.h>
#include <pool.h>

/*  ----------------------------------- DSP/BIOS LINK Headers       */
#include <failure.h>
#include <dsplink.h>
#include <platform.h>
#include <notify.h>
#include <bcache.h>
/*  ----------------------------------- Sample Headers              */
#include <pool_notify_config.h>
#include <task.h>



//for use of the fixed point conversion and calculations
# include "IQmath.h"

#include "math.h"

#define X_AXIS      1       /* Rotation axis.            */
#define Y_AXIS      2
#define Z_AXIS      3

#define FUNC_TRIPEINTERSECT         1
#define FUNC_SPHPEINTERSECT         2
#define FUNC_POLYPEINTERSECT        3
#define FUNC_UNKNOWN_PEINTERSECT    4

#define RAYEPS      1e-07       /* Roundoff error tolerance.         */
#define HUGE_REAL   0x0FFFFFFF       /* A number we consider infinity.    */

#define INSIDE(x, a)        (((a) < 0 && (x) > (a) && (x) < 0) || ((a)  >  0 && (x) > 0 && (x) < (a)))

#define VecDot(A, B)        ( _IQmpy(A[0],B[0]) + _IQmpy(A[1],B[1]) + _IQmpy(A[2],B[2]) )

#define VecSub(C, A, B)     { C[0] = A[0] - B[0]; \
                  C[1] = A[1] - B[1]; \
                  C[2] = A[2] - B[2]; }
                
extern Uint16 MPCSXFER_BufferSize ;

Uint32 stop;


static Void Task_notify (Uint32 eventNo, Ptr arg, Ptr info) ;

Int Task_create (Task_TransferInfo ** infoPtr)
{
    Int status    = SYS_OK ;
    Task_TransferInfo * info = NULL ;

    /* Allocate Task_TransferInfo structure that will be initialized
     * and passed to other phases of the application */
    if (status == SYS_OK) {
        *infoPtr = MEM_calloc (DSPLINK_SEGID,
                               sizeof (Task_TransferInfo),
                               0) ; /* No alignment restriction */
        if (*infoPtr == NULL) {
            status = SYS_EALLOC ;
        }
        else {
            info = *infoPtr ;
        }
    }

    /* Fill up the transfer info structure */
    if (status == SYS_OK) {
        info->dataBuf       = NULL ; /* Set through notification callback. */
        info->bufferSize    = MPCSXFER_BufferSize ;
        SEM_new (&(info->notifySemObj), 0) ;
    }

    /*
     *  Register notification for the event callback to get control and data
     *  buffer pointers from the GPP-side.
     */
    if (status == SYS_OK) {
        status = NOTIFY_register (ID_GPP,
                                  MPCSXFER_IPS_ID,
                                  MPCSXFER_IPS_EVENTNO,
                                  (FnNotifyCbck) Task_notify,
                                  info) ;
        if (status != SYS_OK) {
            return status;
        }
    }

    /*
     *  Send notification to the GPP-side that the application has completed its
     *  setup and is ready for further execution.
     */
    if (status == SYS_OK) {
        status = NOTIFY_notify (ID_GPP,
                                MPCSXFER_IPS_ID,
                                MPCSXFER_IPS_EVENTNO,
                                (Uint32) 0) ; /* No payload to be sent. */
        if (status != SYS_OK) {
            return status;
        }
    }

    /*
     *  Wait for the event callback from the GPP-side to post the semaphore
     *  indicating receipt of the data buffer pointer and image width and height.
     */
    SEM_pend (&(info->notifySemObj), SYS_FOREVER) ;
    SEM_pend (&(info->notifySemObj), SYS_FOREVER) ;

    return status ;
}

Uint32* buf;
int length;


/*
 *  The execute function does the calculations with the input of the ARM.
 *
 *  The loop can be stopped by receiving a notification with value 0.
 */
Int Task_execute (Task_TransferInfo * info)
{
    _iq    q1, q2;
    _iq    tval;           /* Intersection t distance value.    */
    _iq    v1[3], v2[3], v3[3];      /* Vertex list pointers.         */
    _iq    e1[3], e2[3], e3[3];     /* Edge vectors.             */
    _iq  P[3], D[3];
    _iq  norm[3];
    Uint32  indx;
    _iq  Rd_dot_Pn, Ro_dot_Pn;
    _iq  d, center[3], rad2;
    _iq  V[3];
    _iq  vsq, b, disc;
    _iq  vpos[12];
    _iq  I[3];
    _iq iy,ix;
    _iq x[4],y[4],dx,dy;
    _iq xint;
    _iq t_out;
    
    Uint32 sh, nsh,toright;
    Uint32 axis_proj, nverts;
    Uint32 numIterations;
    Uint32 callerCode;
    char function;
    Uint32 curr;
    Uint32 SizeOfData;

    
    Uint32 i,j,hit;
    _iq b1, b2, b3,tval_old;
    
    stop = 1;
 
    while (stop){
        SEM_pend (&(info->notifySemObj), SYS_FOREVER);

        BCACHE_inv ((Ptr)buf, length, TRUE) ;
        
        i=0;
        hit=0;
        numIterations   = buf[i++];
        callerCode      = buf[i++];
        curr            = buf[i++];
        SizeOfData      = buf[i++];
        //t_out           = _IQ(buf[i++]);
        i++;
                
        tval_old = MAX_IQ_POS;
        for(j=0;j<numIterations;j++){
            
            //check which function needs to be called; Tri,Sphere or Poly
            function = buf[i++];
            
            
            
            // the original TriPeIntersect() function is edited to make it run on the DSP
            if (function == FUNC_TRIPEINTERSECT)
            {
                P[0] = (buf[i++]);
                P[1] = (buf[i++]);
                P[2] = (buf[i++]);
                D[0] = (buf[i++]);
                D[1] = (buf[i++]);
                D[2] = (buf[i++]);
                norm[0] = (buf[i++]);
                norm[1] = (buf[i++]);
                norm[2] = (buf[i++]);
                v1[0] = (buf[i++]);
                v1[1] = (buf[i++]);
                v1[2] = (buf[i++]);
                v2[0] = (buf[i++]);
                v2[1] = (buf[i++]);
                v2[2] = (buf[i++]);
                v3[0] = (buf[i++]);
                v3[1] = (buf[i++]);
                v3[2] = (buf[i++]);
                d     = (buf[i++]);
                indx  = (buf[i++]); 
                
//calc
                hit = 1;                
                Rd_dot_Pn = VecDot(norm, D);

                if (_abs(Rd_dot_Pn) < _IQ(RAYEPS))      /* Ray is parallel.      */
                    hit = 0;
                
                else
                {
                Ro_dot_Pn = VecDot(norm, P);
                tval = - _IQdiv((d + Ro_dot_Pn),Rd_dot_Pn);    /* Intersection distance.    */
                if (tval < _IQ(RAYEPS))            /* Intersects behind ray.    */
                    hit = 0;
                    else
                    {
                        e1[0] = (v2)[0] - (v1)[0];
                        e1[1] = (v2)[1] - (v1)[1];
                        e1[2] = (v2)[2] - (v1)[2];

                        e2[0] = (v3)[0] - (v2)[0];
                        e2[1] = (v3)[1] - (v2)[1];
                        e2[2] = (v3)[2] - (v2)[2];

                        e3[0] = (v1)[0] - (v3)[0];
                        e3[1] = (v1)[1] - (v3)[1];
                        e3[2] = (v1)[2] - (v3)[2];
                        
                        /* Triangle containment. */
                        if(indx==1)
                        {
                            q1 = P[1] + _IQmpy(tval,D[1]);
                            q2 = P[2] + _IQmpy(tval,D[2]);

                            b1 = (_IQmpy(e2[1] , (q2 - (v2)[2])) - _IQmpy(e2[2] , (q1 - (v2)[1])));
                            b2 = (_IQmpy(e3[1] , (q2 - (v3)[2])) - _IQmpy(e3[2] , (q1 - (v3)[1])));
                            b3 = (_IQmpy(e1[1] , (q2 - (v1)[2])) - _IQmpy(e1[2] , (q1 - (v1)[1])));
                            if (!INSIDE((b1), norm[0]))
                                hit = 0;
                            if (!INSIDE((b2), norm[0]))
                                hit = 0;                            
                            if (!INSIDE((b3), norm[0]))
                                hit = 0;
                            }
                        else if (indx == 2)
                        {
                            q1 = P[0] + _IQmpy(tval,D[0]);
                            q2 = P[2] + _IQmpy(tval,D[2]);
                            
                            b1 = (_IQmpy(e2[2] , (q1 - (v2)[0])) - _IQmpy(e2[0] , (q2 - (v2)[2])));
                            b2 = (_IQmpy(e3[2] , (q1 - (v3)[0])) - _IQmpy(e3[0] , (q2 - (v3)[2])));
                            b3 = (_IQmpy(e1[2] , (q1 - (v1)[0])) - _IQmpy(e1[0] , (q2 - (v1)[2])));
                            if (!INSIDE((b1), norm[1]))
                                hit = 0;

                            
                            if (!INSIDE((b2), norm[1]))
                                hit = 0;

                            
                            if (!INSIDE((b3), norm[1]))
                                hit = 0;
                        }
                        else if (indx == 3)
                        {
                            q1 = P[0] + _IQmpy(tval,D[0]);
                            q2 = P[1] + _IQmpy(tval,D[1]);

                            b1 = (_IQmpy(e2[0] , (q2 - (v2)[1])) - _IQmpy(e2[1] , (q1 - (v2)[0])));
                            b2 = (_IQmpy(e3[0] , (q2 - (v3)[1])) - _IQmpy(e3[1] , (q1 - (v3)[0])));
                            b3 = (_IQmpy(e1[0] , (q2 - (v1)[1])) - _IQmpy(e1[1] , (q1 - (v1)[0])));
                            if (!INSIDE((b1), norm[2]))
                                hit = 0;

                            
                            if (!INSIDE((b2), norm[2]))
                                hit = 0;

                            
                            if (!INSIDE((b3), norm[2]))
                                hit = 0;
                        }
                        else
                            hit = 0;
                    }
                }

                buf[ SizeOfData +(j*10)] = hit;
                
                if (hit)
                    //if (tval < tval_old && tval < t_out)
                   // {
                   //     tval_old = tval;
                        buf[ SizeOfData+1 +(j*10)] = (tval);
                        buf[ SizeOfData+2 +(j*10)] = j;                        
                        buf[ SizeOfData+3 +(j*10)] = (b1);
                        buf[ SizeOfData+4 +(j*10)] = (b2);
                        buf[ SizeOfData+5 +(j*10)] = (b3);
                   // }
                    
                    
                    
                    
            }
            
            
            // the original SphPeIntersect() function is edited to make it run on the DSP
            else if (function == FUNC_SPHPEINTERSECT)
            {
                
                P[0] = (buf[i++]);
                P[1] = (buf[i++]);
                P[2] = (buf[i++]);
                D[0] = (buf[i++]);
                D[1] = (buf[i++]);
                D[2] = (buf[i++]);
                center[0] = (buf[i++]);
                center[1] = (buf[i++]);
                center[2] = (buf[i++]);
                rad2 = (buf[i++]);

                VecSub(V, center, P);       /* Ray from origin to center.*/
                vsq = VecDot(V, V);         /* Length sq of V.       */
                b   = VecDot(V, D);         /* Perpendicular scale of V. */

                disc = _IQmpy(b,b) - vsq + rad2;        /* Discriminate.         */

                if (disc < 0)             /* Misses ray.           */
                    hit = 0;
                else {

                    disc = _IQsqrt(disc);          /* Find intersection param.  */
                    tval   = b + disc;
                    
                    hit = 1;

                    //if (tval < tval_old && tval < t_out)
                    //{
                    //    tval_old = tval;
                        
                        buf[ SizeOfData+1+(j*10) ] = (tval);
                        buf[ SizeOfData+2+(j*10) ] = j; 
                    //}
                }
                buf[ SizeOfData +(j*10)] = hit;
            }
            
            
            // the original PolyPeIntersect() function is edited to make it run on the DSP
            else if (function == FUNC_POLYPEINTERSECT)
            {
                P[0] = (buf[i++]);
                P[1] = (buf[i++]);
                P[2] = (buf[i++]);
                D[0] = (buf[i++]);
                D[1] = (buf[i++]);
                D[2] = (buf[i++]);
                d    = (buf[i++]);
                norm[0] = (buf[i++]);
                norm[1] = (buf[i++]);
                norm[2] = (buf[i++]);
                vpos[0] = (buf[i++]);
                vpos[1] = (buf[i++]);
                vpos[2] = (buf[i++]);                
                vpos[3] = (buf[i++]);
                vpos[4] = (buf[i++]);
                vpos[5] = (buf[i++]);                
                vpos[6] = (buf[i++]);
                vpos[7] = (buf[i++]);
                vpos[8] = (buf[i++]);                
                vpos[9] = (buf[i++]);
                vpos[10] = (buf[i++]);
                vpos[11] = (buf[i++]);
                axis_proj = (Uint32)(buf[i++]);
                nverts  = (Uint32)(buf[i++]);
    
                hit = 1;                
                Rd_dot_Pn = VecDot(norm, D);

                if (_abs(Rd_dot_Pn) < _IQ(RAYEPS))      /* Ray is parallel.      */
                    hit = 0;                
                else
                {
                    Ro_dot_Pn = VecDot(norm, P);
                    tval = - _IQdiv((d + Ro_dot_Pn),Rd_dot_Pn);    /* Intersection distance.    */
                    if (tval < _IQ(RAYEPS))            /* Intersects behind ray.    */
                        hit = 0;
                    else
                    {   
                        I[0] = P[0] + _IQmpy(D[0],tval);
                        I[1] = P[1] + _IQmpy(D[1],tval);
                        I[2] = P[2] + _IQmpy(D[2],tval);   
                        

                        /* Polygon containment. */
                        /* Project onto plane with greatest normal component. */
                        switch (axis_proj)
                        {
                        case X_AXIS:
                            for (i = 0; i < nverts; i++)
                            {
                                x[i] = (vpos)[1+(i*3)];
                                y[i] = (vpos)[2+(i*3)];

                            }

                            ix = I[1];
                            iy = I[2];
                            break;

                        case Y_AXIS:
                            for (i = 0; i < nverts; i++)
                            {
                                x[i] = (vpos)[0+(i*3)];
                                y[i] = (vpos)[2+(i*3)];
                            }

                            ix = I[0];
                            iy = I[2];
                            break;

                        case Z_AXIS:
                            for (i = 0; i < nverts; i++)
                            {
                                x[i] = (vpos)[0+(i*3)];
                                y[i] = (vpos)[1+(i*3)];
                            }

                            ix = I[0];
                            iy = I[1];
                            break;
                        }


                        /* Translate to origin. */

                        for (i = 0; i < nverts; i++)
                        {
                            x[i] -= ix;
                            y[i] -= iy;

                            if (_abs(y[i]) < _IQ(RAYEPS))
                                y[i] = 0;
                        }

                        x[nverts] = x[0];
                        y[nverts] = y[0];


                        /*
                        *  If intersection point crosses an odd number of line segments,
                        *  the point is inside the polygon
                        */


                        if (y[0] < 0)
                            sh = 0;
                        else
                            sh = 1;

                        toright = 0;

                        for (i = 0; i < nverts; i++)
                        {
                            /* Check if segment crosses in y. */

                            if (y[i + 1] < 0)
                                nsh = 0;
                            else
                                nsh = 1;

                            if (nsh ^ sh)                       
                            {
                                dy = y[i + 1] - y[i];

                                if (_abs(dy) >= _IQ(RAYEPS))
                                {
                                    dx   = x[i + 1] - x[i];
                                    xint = x[i] - _IQdiv(_IQmpy(y[i],dx) , dy);

                                    if (xint > 0)
                                        toright++;
                                }
                            }

                            sh = nsh;
                        }

                        if (toright%2 == 1)
                        {
                            if (hit)
                                //if (tval < tval_old && tval < t_out)
                                //{
                                //    tval_old = tval;
                                    buf[ SizeOfData+1 +(j*10)] = (tval);
                                    buf[ SizeOfData+2 +(j*10)] = j; 
                                //}  

                                buf[ SizeOfData +(j*10)] = hit;
                        }
                        else
                            buf[ SizeOfData +(j*10)] = 0;
                    }
                }
            }
            else if (function == FUNC_UNKNOWN_PEINTERSECT)
            {}// does nothing.
         }
         
         
        // copy data and notify the ARM.
        BCACHE_wb ((Ptr)buf, length, TRUE) ;        
        NOTIFY_notify(ID_GPP,MPCSXFER_IPS_ID,MPCSXFER_IPS_EVENTNO,1);
    
    }

    return SYS_OK;
}

Int Task_delete (Task_TransferInfo * info)
{
    Int    status     = SYS_OK ;
    /*
     *  Unregister notification for the event callback used to get control and
     *  data buffer pointers from the GPP-side.
     */
    status = NOTIFY_unregister (ID_GPP,
                                MPCSXFER_IPS_ID,
                                MPCSXFER_IPS_EVENTNO,
                                (FnNotifyCbck) Task_notify,
                                info) ;

    /* Free the info structure */
    MEM_free (DSPLINK_SEGID,
              info,
              sizeof (Task_TransferInfo)) ;
    info = NULL ;

    return status ;
}


static Void Task_notify (Uint32 eventNo, Ptr arg, Ptr info)
{
    static Uint32 count = 0;
    Task_TransferInfo * mpcsInfo = (Task_TransferInfo *) arg ;

    (Void) eventNo ; /* To avoid compiler warning. */

    count++;
    if (count==1) {
        buf =(Uint32*)info ;
    }
    if (count==2) {
        length = (int)info;
    }
    else
        stop = (Uint32)info; //check bit to end the while loop.

    SEM_post(&(mpcsInfo->notifySemObj));
}
